/*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014-2015, Timm Linder, Social Robotics Lab, University of Freiburg
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions are met:
*
*  * Redistributions of source code must retain the above copyright notice, this
*    list of conditions and the following disclaimer.
*  * Redistributions in binary form must reproduce the above copyright notice,
*    this list of conditions and the following disclaimer in the documentation
*    and/or other materials provided with the distribution.
*  * Neither the name of the copyright holder nor the names of its contributors
*    may be used to endorse or promote products derived from this software
*    without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#ifndef _LOW_CONFIDENCE_OBSERVATIONS_RECOVERY_H
#define _LOW_CONFIDENCE_OBSERVATIONS_RECOVERY_H

#include <ros/ros.h>
#include <srl_nearest_neighbor_tracker/missed_observation_recovery/missed_observation_recovery.h>
#include <boost/shared_ptr.hpp>


namespace srl_nnt {

/// Subscribes to a DetectedPersons topic containing low-confidence detections that an unmatched track can additionally be matched against.
class LowConfidenceObservationsRecovery : public MissedObservationRecovery {
public:
    // Virtual destructor
    virtual ~LowConfidenceObservationsRecovery() {}

    /// Initialization method
    virtual void init(ros::NodeHandle& nodeHandle, ros::NodeHandle& privateNodeHandle);

    /// Tries to recover observations by searching the separately subscribed DetectedPersons topic for matching close-by observations.
    virtual void recoverObservations(const double currentTime, const Tracks& unmatchedTracks, const Tracks& matchedTracks, Observations& recoveredObservations);

    /// Returns a name of this recovery mechanism. Must be ROS-topic compatible, i.e. not contain spaces.
    virtual const std::string getName() { return "low_confidence_observations"; };

protected:
    /// Message callback
    void onNewLowConfidenceDetectionsReceived(const spencer_tracking_msgs::DetectedPersons::ConstPtr detectedPersons);

    ros::Subscriber m_additionalLowConfidenceDetectionsSubscriber;
    spencer_tracking_msgs::DetectedPersons::ConstPtr m_currentLowConfidenceDetections;
    bool m_syncWarningShown;
    int m_numCyclesDroppedSoFar, m_numCyclesTotal;
};

}

#endif //_LOW_CONFIDENCE_OBSERVATIONS_RECOVERY_H
